#include "bsp_mpu6050.h"
#include <stdio.h>

void mpu6050_init() {

  /* IO初始化*/
  IOMUXC_SetPinMux(IOMUXC_UART4_TX_DATA_I2C1_SCL, 1);
  IOMUXC_SetPinMux(IOMUXC_UART4_RX_DATA_I2C1_SDA, 1);
  IOMUXC_SetPinConfig(IOMUXC_UART4_TX_DATA_I2C1_SCL, 0x70b0);
  IOMUXC_SetPinConfig(IOMUXC_UART4_RX_DATA_I2C1_SDA, 0x70b0);
  /* I2C初始化*/
  i2c_init(I2C1);
  /* mpu6050初始化*/
  /*MPU6050寄存器初始化，需要对照MPU6050手册的寄存器描述配置，此处仅配置了部分重要的寄存器*/
  MPU6050_WriteReg(
      MPU6050_PWR_MGMT_1,
      0x01); // 电源管理寄存器1，取消睡眠模式，选择时钟源为X轴陀螺仪
  MPU6050_WriteReg(MPU6050_PWR_MGMT_2,
                   0x00); // 电源管理寄存器2，保持默认值0，所有轴均不待机
  MPU6050_WriteReg(MPU6050_SMPLRT_DIV, 0x09); // 采样率分频寄存器，配置采样率
  MPU6050_WriteReg(MPU6050_CONFIG, 0x06); // 配置寄存器，配置DLPF
  MPU6050_WriteReg(MPU6050_GYRO_CONFIG,
                   0x18); // 陀螺仪配置寄存器，选择满量程为±2000°/s
  MPU6050_WriteReg(MPU6050_ACCEL_CONFIG,
                   0x18); // 加速度计配置寄存器，选择满量程为±16g
}

/**
 * 函    数：MPU6050写寄存器
 * 参    数：RegAddress 寄存器地址，范围：参考MPU6050手册的寄存器描述
 * 参    数：Data 要写入寄存器的数据，范围：0x00~0xFF
 * 返 回 值：无
 */
unsigned char MPU6050_WriteReg(unsigned char reg, unsigned char data) {
  unsigned char writedata = data;

  struct i2c_transfer xfer;

  xfer.slaveAddress = MPU6050_ADDRESS;
  xfer.direction = kI2C_Write;
  xfer.subaddress = reg;
  xfer.subaddressSize = 1;
  xfer.data = &writedata;
  xfer.dataSize = 1;

  return i2c_master_transfer(I2C1, &xfer);
}

/**
 * 函    数：MPU6050获取ID号
 * 参    数：无
 * 返 回 值：MPU6050的ID号
 */
uint8_t MPU6050_GetID(void) {
  return MPU6050_ReadReg(MPU6050_WHO_AM_I); // 返回WHO_AM_I寄存器的值
}

/**
 * @brief MPU6050读寄存器
 *
 * @param reg 寄存器地址
 * @return unsigned char 寄存器的值
 */
unsigned char MPU6050_ReadReg(unsigned char reg) {

  unsigned char val = 0;
  struct i2c_transfer xfer;

  xfer.slaveAddress = MPU6050_ADDRESS;
  xfer.direction = kI2C_Read;
  xfer.subaddress = reg;
  xfer.subaddressSize = 1;
  xfer.data = &val;
  xfer.dataSize = 1;

  i2c_master_transfer(I2C1, &xfer);

  return val;
}
/**
 * 函    数：MPU6050获取数据
 * 参    数：AccX AccY AccZ
 * 加速度计X、Y、Z轴的数据，使用输出参数的形式返回，范围：-32768~32767 参
 * 数：GyroX GyroY GyroZ
 * 陀螺仪X、Y、Z轴的数据，使用输出参数的形式返回，范围：-32768~32767 返 回
 * 值：无
 */
void MPU6050_GetData(int16_t *AccX, int16_t *AccY, int16_t *AccZ,
                     int16_t *GyroX, int16_t *GyroY, int16_t *GyroZ) {
  uint8_t DataH, DataL; // 定义数据高8位和低8位的变量

  DataH = MPU6050_ReadReg(MPU6050_ACCEL_XOUT_H); // 读取加速度计X轴的高8位数据
  DataL = MPU6050_ReadReg(MPU6050_ACCEL_XOUT_L); // 读取加速度计X轴的低8位数据
  *AccX = (DataH << 8) | DataL; // 数据拼接，通过输出参数返回

  DataH = MPU6050_ReadReg(MPU6050_ACCEL_YOUT_H); // 读取加速度计Y轴的高8位数据
  DataL = MPU6050_ReadReg(MPU6050_ACCEL_YOUT_L); // 读取加速度计Y轴的低8位数据
  *AccY = (DataH << 8) | DataL; // 数据拼接，通过输出参数返回

  DataH = MPU6050_ReadReg(MPU6050_ACCEL_ZOUT_H); // 读取加速度计Z轴的高8位数据
  DataL = MPU6050_ReadReg(MPU6050_ACCEL_ZOUT_L); // 读取加速度计Z轴的低8位数据
  *AccZ = (DataH << 8) | DataL; // 数据拼接，通过输出参数返回

  DataH = MPU6050_ReadReg(MPU6050_GYRO_XOUT_H); // 读取陀螺仪X轴的高8位数据
  DataL = MPU6050_ReadReg(MPU6050_GYRO_XOUT_L); // 读取陀螺仪X轴的低8位数据
  *GyroX = (DataH << 8) | DataL; // 数据拼接，通过输出参数返回

  DataH = MPU6050_ReadReg(MPU6050_GYRO_YOUT_H); // 读取陀螺仪Y轴的高8位数据
  DataL = MPU6050_ReadReg(MPU6050_GYRO_YOUT_L); // 读取陀螺仪Y轴的低8位数据
  *GyroY = (DataH << 8) | DataL; // 数据拼接，通过输出参数返回

  DataH = MPU6050_ReadReg(MPU6050_GYRO_ZOUT_H); // 读取陀螺仪Z轴的高8位数据
  DataL = MPU6050_ReadReg(MPU6050_GYRO_ZOUT_L); // 读取陀螺仪Z轴的低8位数据
  *GyroZ = (DataH << 8) | DataL; // 数据拼接，通过输出参数返回
}